#include "../inc/m_modbus_pnp.h"

// 全局上下文实例
static m_modbus_ctx_t g_modbus_ctx;

/**
 * @brief 解析配置文件（ini格式）
 * @return 成功返回0，失败返回-1
 */
static int parse_config(void) {
    FILE *fp = fopen(CONFIG_FILE_PATH, "r");
    if (!fp) {
        fprintf(stderr, "Failed to open config file: %s\n", CONFIG_FILE_PATH);
        return -1;
    }

    char line[256], section[32], key[32], val[64];
    memset(section, 0, sizeof(section));

    // 默认配置
    g_modbus_ctx.config.mode = MODBUS_MODE_RTU;
    strcpy(g_modbus_ctx.config.serial_dev, "/dev/ttyS0");
    g_modbus_ctx.config.baudrate = 9600;
    g_modbus_ctx.config.data_bits = 8;
    g_modbus_ctx.config.stop_bits = 1;
    g_modbus_ctx.config.parity = 'N';
    g_modbus_ctx.config.slave_addr = 1;
    strcpy(g_modbus_ctx.config.tcp_ip, "0.0.0.0");
    g_modbus_ctx.config.tcp_port = 502;
    g_modbus_ctx.config.is_server = 1;

    while (fgets(line, sizeof(line), fp)) {
        // 去除换行符
        line[strcspn(line, "\n")] = '\0';
        // 跳过空行和注释
        if (line[0] == '#' || line[0] == '\0' || line[0] == ';') continue;

        // 解析section（[SECTION]）
        if (line[0] == '[' && line[strlen(line)-1] == ']') {
            sscanf(line, "[%[^]]", section);
            continue;
        }

        // 解析key=val
        if (sscanf(line, "%[^=]=%[^\n]", key, val) != 2) continue;

        // 处理COMM配置
        if (strcmp(section, "MODBUS_COMM") == 0) {
            if (strcmp(key, "TYPE") == 0) {
                g_modbus_ctx.config.mode = (strcmp(val, "TCP") == 0) ? MODBUS_MODE_TCP : MODBUS_MODE_RTU;
            } else if (strcmp(key, "SERIAL_DEV") == 0) {
                strncpy(g_modbus_ctx.config.serial_dev, val, sizeof(g_modbus_ctx.config.serial_dev)-1);
            } else if (strcmp(key, "BAUDRATE") == 0) {
                g_modbus_ctx.config.baudrate = atoi(val);
            } else if (strcmp(key, "DATA_BITS") == 0) {
                g_modbus_ctx.config.data_bits = atoi(val);
            } else if (strcmp(key, "STOP_BITS") == 0) {
                g_modbus_ctx.config.stop_bits = atoi(val);
            } else if (strcmp(key, "PARITY") == 0) {
                g_modbus_ctx.config.parity = val[0];
            } else if (strcmp(key, "SLAVE_ADDR") == 0) {
                g_modbus_ctx.config.slave_addr = atoi(val);
            } else if (strcmp(key, "TCP_IP") == 0) {
                strncpy(g_modbus_ctx.config.tcp_ip, val, sizeof(g_modbus_ctx.config.tcp_ip)-1);
            } else if (strcmp(key, "TCP_PORT") == 0) {
                g_modbus_ctx.config.tcp_port = atoi(val);
            } else if (strcmp(key, "IS_SERVER") == 0) {
                g_modbus_ctx.config.is_server = (strcmp(val, "1") == 0) ? 1 : 0;
            }
        }
    }
    fclose(fp);
    return 0;
}

/**
 * @brief 查找寄存器（按类型和地址）
 * @param type 寄存器类型
 * @param addr 寄存器地址
 * @return 找到返回寄存器指针，未找到返回NULL
 */
static modbus_register_t *find_register(modbus_reg_type_t type, uint16_t addr) {
    for (int i = 0; i < g_modbus_ctx.reg_count; i++) {
        modbus_register_t *reg = &g_modbus_ctx.regs[i];
        if (reg->type == type && addr >= reg->addr && addr < reg->addr + reg->len) {
            return reg;
        }
    }
    return NULL;
}

/**
 * @brief 线圈读取回调（功能码0x01）
 */
static int coil_read_cb(modbus_t *ctx, const uint8_t *req, int req_len, uint8_t *resp, int *resp_len) {
    (void)ctx;
    uint16_t addr = (req[1] << 8) | req[2];
    uint16_t len = (req[3] << 8) | req[4];

    // 查找寄存器
    modbus_register_t *reg = find_register(REG_TYPE_COIL, addr);
    if (!reg || reg->access == ACCESS_MAX) {
        *resp_len = modbus_build_exception_response(req, MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, resp);
        return -1;
    }

    // 计算偏移量
    uint16_t offset = addr - reg->addr;
    if (offset + len > reg->len) {
        *resp_len = modbus_build_exception_response(req, MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, resp);
        return -1;
    }

    // 填充响应数据（线圈按字节打包，1bit=1个线圈）
    pthread_mutex_lock(&g_modbus_ctx.reg_mutex);
    int byte_len = (len + 7) / 8;
    resp[0] = req[0]; // 从站地址
    resp[1] = req[1]; // 功能码
    resp[2] = byte_len; // 数据长度
    memset(&resp[3], 0, byte_len);
    for (int i = 0; i < len; i++) {
        int byte_idx = (offset + i) / 8;
        int bit_idx = (offset + i) % 8;
        resp[3 + byte_idx] |= (reg->data.coil[offset + i] & 0x01) << (7 - bit_idx);
    }
    pthread_mutex_unlock(&g_modbus_ctx.reg_mutex);

    *resp_len = 3 + byte_len;
    return 0;
}

/**
 * @brief 线圈写入回调（功能码0x05）
 */
static int coil_write_cb(modbus_t *ctx, const uint8_t *req, int req_len, uint8_t *resp, int *resp_len) {
    (void)ctx;
    uint16_t addr = (req[1] << 8) | req[2];
    uint16_t val = (req[3] << 8) | req[4];
    val = (val == 0x0000) ? 0 : 1; // 线圈值仅0或1

    // 查找寄存器
    modbus_register_t *reg = find_register(REG_TYPE_COIL, addr);
    if (!reg || reg->access != ACCESS_READ_WRITE) {
        *resp_len = modbus_build_exception_response(req, MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, resp);
        return -1;
    }

    // 计算偏移量
    uint16_t offset = addr - reg->addr;
    if (offset >= reg->len) {
        *resp_len = modbus_build_exception_response(req, MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, resp);
        return -1;
    }

    // 更新线圈值
    pthread_mutex_lock(&g_modbus_ctx.reg_mutex);
    reg->data.coil[offset] = val;
    // 触发写回调
    if (reg->write_cb) {
        reg->write_cb(REG_TYPE_COIL, addr, val);
    }
    pthread_mutex_unlock(&g_modbus_ctx.reg_mutex);

    // 响应：回显请求
    memcpy(resp, req, 6);
    *resp_len = 6;
    return 0;
}

/**
 * @brief 保持寄存器读取回调（功能码0x03）
 */
static int holding_read_cb(modbus_t *ctx, const uint8_t *req, int req_len, uint8_t *resp, int *resp_len) {
    (void)ctx;
    uint16_t addr = (req[1] << 8) | req[2];
    uint16_t len = (req[3] << 8) | req[4];

    // 查找寄存器
    modbus_register_t *reg = find_register(REG_TYPE_HOLDING, addr);
    if (!reg || reg->access == ACCESS_MAX) {
        *resp_len = modbus_build_exception_response(req, MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, resp);
        return -1;
    }

    // 计算偏移量
    uint16_t offset = addr - reg->addr;
    if (offset + len > reg->len) {
        *resp_len = modbus_build_exception_response(req, MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, resp);
        return -1;
    }

    // 填充响应数据（16bit/寄存器，大端序）
    pthread_mutex_lock(&g_modbus_ctx.reg_mutex);
    resp[0] = req[0]; // 从站地址
    resp[1] = req[1]; // 功能码
    resp[2] = len * 2; // 数据长度（字节）
    for (int i = 0; i < len; i++) {
        resp[3 + 2*i] = (reg->data.holding[offset + i] >> 8) & 0xFF;
        resp[4 + 2*i] = reg->data.holding[offset + i] & 0xFF;
    }
    pthread_mutex_unlock(&g_modbus_ctx.reg_mutex);

    *resp_len = 3 + len * 2;
    return 0;
}

/**
 * @brief 保持寄存器写入回调（功能码0x06）
 */
static int holding_write_cb(modbus_t *ctx, const uint8_t *req, int req_len, uint8_t *resp, int *resp_len) {
    (void)ctx;
    uint16_t addr = (req[1] << 8) | req[2];
    uint16_t val = (req[3] << 8) | req[4];

    // 查找寄存器
    modbus_register_t *reg = find_register(REG_TYPE_HOLDING, addr);
    if (!reg || reg->access != ACCESS_READ_WRITE) {
        *resp_len = modbus_build_exception_response(req, MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, resp);
        return -1;
    }

    // 计算偏移量
    uint16_t offset = addr - reg->addr;
    if (offset >= reg->len) {
        *resp_len = modbus_build_exception_response(req, MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, resp);
        return -1;
    }

    // 更新寄存器值
    pthread_mutex_lock(&g_modbus_ctx.reg_mutex);
    reg->data.holding[offset] = val;
    // 触发写回调
    if (reg->write_cb) {
        reg->write_cb(REG_TYPE_HOLDING, addr, val);
    }
    pthread_mutex_unlock(&g_modbus_ctx.reg_mutex);

    // 响应：回显请求
    memcpy(resp, req, 6);
    *resp_len = 6;
    return 0;
}

/**
 * @brief 注册标准功能码回调
 */
static int register_callbacks(void) {
    // 线圈读写
    if (modbus_register_cb(g_modbus_ctx.ctx, MODBUS_FC_READ_COILS, coil_read_cb) != 0) {
        fprintf(stderr, "Register read coils callback failed\n");
        return -1;
    }
    if (modbus_register_cb(g_modbus_ctx.ctx, MODBUS_FC_WRITE_SINGLE_COIL, coil_write_cb) != 0) {
        fprintf(stderr, "Register write coil callback failed\n");
        return -1;
    }

    // 保持寄存器读写
    if (modbus_register_cb(g_modbus_ctx.ctx, MODBUS_FC_READ_HOLDING_REGISTERS, holding_read_cb) != 0) {
        fprintf(stderr, "Register read holding callback failed\n");
        return -1;
    }
    if (modbus_register_cb(g_modbus_ctx.ctx, MODBUS_FC_WRITE_SINGLE_REGISTER, holding_write_cb) != 0) {
        fprintf(stderr, "Register write holding callback failed\n");
        return -1;
    }

    // 可扩展：添加离散输入（0x02）、输入寄存器（0x04）、多寄存器写入（0x10）等回调
    return 0;
}

/**
 * @brief 初始化Modbus上下文（RTU/TCP）
 */
static int modbus_ctx_init(void) {
    // 销毁旧上下文
    if (g_modbus_ctx.ctx) {
        modbus_close(g_modbus_ctx.ctx);
        modbus_free(g_modbus_ctx.ctx);
        g_modbus_ctx.ctx = NULL;
    }

    // 根据模式创建上下文
    if (g_modbus_ctx.config.mode == MODBUS_MODE_RTU) {
        g_modbus_ctx.ctx = modbus_new_rtu(
                g_modbus_ctx.config.serial_dev,
                g_modbus_ctx.config.baudrate,
                g_modbus_ctx.config.parity,
                g_modbus_ctx.config.data_bits,
                g_modbus_ctx.config.stop_bits
        );
    } else {
        g_modbus_ctx.ctx = modbus_new_tcp(
                g_modbus_ctx.config.tcp_ip,
                g_modbus_ctx.config.tcp_port
        );
    }

    if (!g_modbus_ctx.ctx) {
        fprintf(stderr, "Create modbus context failed: %s\n", modbus_strerror(errno));
        return -1;
    }

    // 设置从站地址
    modbus_set_slave(g_modbus_ctx.ctx, g_modbus_ctx.config.slave_addr);
    // 设置超时
    modbus_set_response_timeout(g_modbus_ctx.ctx, 0, MODBUS_TIMEOUT * 1000);

    // 连接（TCP服务端模式不需要提前连接）
    if (g_modbus_ctx.config.mode == MODBUS_MODE_RTU || !g_modbus_ctx.config.is_server) {
        if (modbus_connect(g_modbus_ctx.ctx) == -1) {
            fprintf(stderr, "Modbus connect failed: %s\n", modbus_strerror(errno));
            modbus_free(g_modbus_ctx.ctx);
            g_modbus_ctx.ctx = NULL;
            return -1;
        }
    }

    // 注册功能码回调
    if (register_callbacks() != 0) {
        modbus_close(g_modbus_ctx.ctx);
        modbus_free(g_modbus_ctx.ctx);
        g_modbus_ctx.ctx = NULL;
        return -1;
    }

    printf("Modbus context init success (mode: %s)\n",
           g_modbus_ctx.config.mode == MODBUS_MODE_RTU ? "RTU" : "TCP");

    // 初始化配置模板（自动配置依赖）
//    m_modbus_load_templates();
    // 初始化连接池（自动连接依赖）
//    conn_pool_init();
    // 初始化自动检测（原有）
    m_modbus_scan_init();

    g_modbus_ctx.running = 1;
    return 0;
}

/**
 * @brief TCP服务端监听初始化
 */
static int tcp_server_init(void) {
    struct sockaddr_in addr;
    int opt = 1;

    // 创建TCP socket
    g_modbus_ctx.conn_fd = socket(AF_INET, SOCK_STREAM, 0);
    if (g_modbus_ctx.conn_fd == -1) {
        fprintf(stderr, "Create TCP socket failed: %s\n", strerror(errno));
        return -1;
    }

    // 设置端口复用
    if (setsockopt(g_modbus_ctx.conn_fd, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt)) == -1) {
        fprintf(stderr, "Setsockopt failed: %s\n", strerror(errno));
        close(g_modbus_ctx.conn_fd);
        return -1;
    }

    // 绑定地址
    memset(&addr, 0, sizeof(addr));
    addr.sin_family = AF_INET;
    addr.sin_addr.s_addr = inet_addr(g_modbus_ctx.config.tcp_ip);
    addr.sin_port = htons(g_modbus_ctx.config.tcp_port);
    if (bind(g_modbus_ctx.conn_fd, (struct sockaddr *)&addr, sizeof(addr)) == -1) {
        fprintf(stderr, "TCP bind failed: %s\n", strerror(errno));
        close(g_modbus_ctx.conn_fd);
        return -1;
    }

    // 开始监听
    if (listen(g_modbus_ctx.conn_fd, 5) == -1) {
        fprintf(stderr, "TCP listen failed: %s\n", strerror(errno));
        close(g_modbus_ctx.conn_fd);
        return -1;
    }

    printf("TCP server listening on %s:%d\n",
           g_modbus_ctx.config.tcp_ip, g_modbus_ctx.config.tcp_port);
    return 0;
}

int m_modbus_init(void) {
    // 初始化全局上下文
    memset(&g_modbus_ctx, 0, sizeof(m_modbus_ctx_t));
    g_modbus_ctx.running = 0;
    g_modbus_ctx.reg_count = 0;

    // 初始化互斥锁
    if (pthread_mutex_init(&g_modbus_ctx.reg_mutex, NULL) != 0) {
        fprintf(stderr, "Init mutex failed\n");
        return -1;
    }

    // 解析配置文件
    if (parse_config() != 0) {
        pthread_mutex_destroy(&g_modbus_ctx.reg_mutex);
        return -1;
    }

    // 初始化Modbus上下文
    if (modbus_ctx_init() != 0) {
        pthread_mutex_destroy(&g_modbus_ctx.reg_mutex);
        return -1;
    }

    // TCP服务端模式初始化监听
    if (g_modbus_ctx.config.mode == MODBUS_MODE_TCP && g_modbus_ctx.config.is_server) {
        if (tcp_server_init() != 0) {
            m_modbus_deinit();
            return -1;
        }
    }

    g_modbus_ctx.running = 1;
    return 0;
}

int m_modbus_register(modbus_register_t *reg) {
    if (!reg || g_modbus_ctx.reg_count >= MAX_REG_NUM) {
        fprintf(stderr, "Register failed: invalid param or full\n");
        return -1;
    }

    // 检查地址冲突
    for (int i = 0; i < g_modbus_ctx.reg_count; i++) {
        modbus_register_t *exist = &g_modbus_ctx.regs[i];
        if (exist->type == reg->type &&
            !(reg->addr + reg->len <= exist->addr || reg->addr >= exist->addr + exist->len)) {
            fprintf(stderr, "Register address conflict: type=%d, addr=%d\n", reg->type, reg->addr);
            return -1;
        }
    }

    // 复制寄存器配置
    memcpy(&g_modbus_ctx.regs[g_modbus_ctx.reg_count], reg, sizeof(modbus_register_t));
    g_modbus_ctx.reg_count++;

    printf("Register success: type=%d, addr=%d, len=%d, access=%d\n",
           reg->type, reg->addr, reg->len, reg->access);
    return 0;
}

static int process_request(int client_fd) {
    uint8_t req[MODBUS_TCP_MAX_ADU_LENGTH];
    uint8_t resp[MODBUS_TCP_MAX_ADU_LENGTH];
    int req_len, resp_len;

    // 设置客户端句柄（TCP模式）
    if (g_modbus_ctx.config.mode == MODBUS_MODE_TCP) {
        modbus_set_socket(g_modbus_ctx.ctx, client_fd);
    }

    // 读取请求
    req_len = modbus_receive(g_modbus_ctx.ctx, req);
    if (req_len == -1) {
        if (errno != EAGAIN && errno != EWOULDBLOCK) {
            fprintf(stderr, "Receive request failed: %s\n", modbus_strerror(errno));
        }
        return -1;
    }

    // 处理请求（触发回调函数）
    resp_len = 0;
    if (modbus_process_request(g_modbus_ctx.ctx, req, req_len, resp, &resp_len) != 0) {
        fprintf(stderr, "Process request failed\n");
        return -1;
    }

    // 发送响应
    if (resp_len > 0) {
        if (modbus_send(g_modbus_ctx.ctx, resp, resp_len) == -1) {
            fprintf(stderr, "Send response failed: %s\n", modbus_strerror(errno));
            return -1;
        }
    }

    return 0;
}

int m_modbus_start(void) {
    if (!g_modbus_ctx.running || !g_modbus_ctx.ctx) {
        fprintf(stderr, "Modbus not initialized\n");
        return -1;
    }

    printf("Modbus PnP service started\n");

    if (g_modbus_ctx.config.mode == MODBUS_MODE_TCP && g_modbus_ctx.config.is_server) {
        // TCP服务端模式：循环接受客户端连接
        while (g_modbus_ctx.running) {
            struct sockaddr_in client_addr;
            socklen_t client_len = sizeof(client_addr);
            int client_fd = accept(g_modbus_ctx.conn_fd, (struct sockaddr *)&client_addr, &client_len);
            if (client_fd == -1) {
                if (errno != EINTR) {
                    fprintf(stderr, "Accept client failed: %s\n", strerror(errno));
                    sleep(RECONNECT_INTERVAL);
                }
                continue;
            }

            printf("Client connected: %s:%d\n",
                   inet_ntoa(client_addr.sin_addr), ntohs(client_addr.sin_port));

            // 处理该客户端请求（单线程：一次处理一个客户端）
            while (g_modbus_ctx.running) {
                if (process_request(client_fd) == -1) {
                    break;
                }
            }

            close(client_fd);
            printf("Client disconnected: %s:%d\n",
                   inet_ntoa(client_addr.sin_addr), ntohs(client_addr.sin_port));
        }
    } else {
        // RTU模式或TCP客户端模式：循环处理请求
        while (g_modbus_ctx.running) {
            if (process_request(-1) == -1) {
                // 重连机制
                fprintf(stderr, "Reconnecting...\n");
                sleep(RECONNECT_INTERVAL);
                modbus_ctx_init();
            }
        }
    }

    return 0;
}

void m_modbus_stop(void) {
    g_modbus_ctx.running = 0;
    printf("Modbus PnP service stopping...\n");

    // 唤醒阻塞的socket操作
    if (g_modbus_ctx.conn_fd != -1) {
        shutdown(g_modbus_ctx.conn_fd, SHUT_RDWR);
    }
    if (g_modbus_ctx.ctx) {
        modbus_close(g_modbus_ctx.ctx);
    }
}

void m_modbus_deinit(void) {
    m_modbus_stop();

    // 释放Modbus上下文
    if (g_modbus_ctx.ctx) {
        modbus_free(g_modbus_ctx.ctx);
        g_modbus_ctx.ctx = NULL;
    }

    // 关闭TCP监听句柄
    if (g_modbus_ctx.conn_fd != -1) {
        close(g_modbus_ctx.conn_fd);
        g_modbus_ctx.conn_fd = -1;
    }

    // 销毁互斥锁
    pthread_mutex_destroy(&g_modbus_ctx.reg_mutex);

    printf("Modbus PnP service deinitialized\n");
}

int m_modbus_read_reg(modbus_reg_type_t type, uint16_t addr, uint16_t *val) {
    if (!val || type >= REG_TYPE_MAX) {
        return -1;
    }

    pthread_mutex_lock(&g_modbus_ctx.reg_mutex);
    modbus_register_t *reg = find_register(type, addr);
    if (!reg) {
        pthread_mutex_unlock(&g_modbus_ctx.reg_mutex);
        return -1;
    }

    uint16_t offset = addr - reg->addr;
    switch (type) {
        case REG_TYPE_COIL:
            *val = reg->data.coil[offset] & 0x01;
            break;
        case REG_TYPE_DISCRETE_INPUT:
            *val = reg->data.discrete[offset] & 0x01;
            break;
        case REG_TYPE_HOLDING:
            *val = reg->data.holding[offset];
            break;
        case REG_TYPE_INPUT:
            *val = reg->data.input[offset];
            break;
        default:
            pthread_mutex_unlock(&g_modbus_ctx.reg_mutex);
            return -1;
    }

    pthread_mutex_unlock(&g_modbus_ctx.reg_mutex);
    return 0;
}

int m_modbus_write_reg(modbus_reg_type_t type, uint16_t addr, uint16_t val) {
    if (type >= REG_TYPE_MAX) {
        return -1;
    }

    pthread_mutex_lock(&g_modbus_ctx.reg_mutex);
    modbus_register_t *reg = find_register(type, addr);
    if (!reg || reg->access != ACCESS_READ_WRITE) {
        pthread_mutex_unlock(&g_modbus_ctx.reg_mutex);
        return -1;
    }

    uint16_t offset = addr - reg->addr;
    switch (type) {
        case REG_TYPE_COIL:
            reg->data.coil[offset] = (val != 0) ? 1 : 0;
            break;
        case REG_TYPE_HOLDING:
            reg->data.holding[offset] = val;
            break;
        default:
            pthread_mutex_unlock(&g_modbus_ctx.reg_mutex);
            return -1;
    }

    // 触发写回调
    if (reg->write_cb) {
        reg->write_cb(type, addr, val);
    }

    pthread_mutex_unlock(&g_modbus_ctx.reg_mutex);
    return 0;
}

